class TestTheoJansen extends Test
{
    private tScale:number;
    private m_offset:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2();
    private m_chassis:Box2D.Dynamics.b2Body;
    private m_wheel:Box2D.Dynamics.b2Body;
    private m_motorJoint:Box2D.Dynamics.Joints.b2RevoluteJoint;
    private m_motorOn:boolean = true;
    private m_motorSpeed:number;

    public constructor(sprite:DebugDraw)
    {
        super(sprite);
        this.createScene();
    }

    private createScene():void
    {
        this.tScale = this.m_physScale * 2;

        this.m_offset.Set(120 / this.m_physScale, 250 / this.m_physScale);
        this.m_motorSpeed = -2.0;
        this.m_motorOn = true;

        let pivot:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(0, -24 / this.tScale);

        let pd:Box2D.Collision.Shapes.b2PolygonShape;
        let cd:Box2D.Collision.Shapes.b2CircleShape;
        let fd:Box2D.Dynamics.b2FixtureDef;
        let bd:Box2D.Dynamics.b2BodyDef;
        let body:Box2D.Dynamics.b2Body;

        for(var i:number = 0; i < 40; i++)
        {
            cd = new Box2D.Collision.Shapes.b2CircleShape(7.5 / this.tScale);
            bd = new Box2D.Dynamics.b2BodyDef();
            bd.type = Box2D.Dynamics.b2Body.b2_dynamicBody;
            bd.position.Set((Math.random() * 630 + 10)/ this.m_physScale, 150/ this.m_physScale);
            body = this.m_world.CreateBody(bd);
            body.CreateFixture2(cd,1);
        }

        {
            pd = new Box2D.Collision.Shapes.b2PolygonShape();
            pd.SetAsBox(75 / this.tScale, 30 / this.tScale);
            fd = new Box2D.Dynamics.b2FixtureDef();
            fd.shape = pd;
            fd.density = 1;
            fd.filter.groupIndex = -1;
            bd = new Box2D.Dynamics.b2BodyDef();
            bd.type = Box2D.Dynamics.b2Body.b2_dynamicBody;
            bd.position = Box2D.Common.Math.b2Math.AddVV(pivot,this.m_offset);
            this.m_chassis = this.m_world.CreateBody(bd);
            this.m_chassis.CreateFixture(fd);
        }
        {
            cd = new Box2D.Collision.Shapes.b2CircleShape(48 / this.tScale);
            fd = new Box2D.Dynamics.b2FixtureDef();
            fd.shape = cd;
            fd.density = 1.0;
            fd.filter.groupIndex = -1;
            bd.position = Box2D.Common.Math.b2Math.AddVV(pivot, this.m_offset);
            this.m_wheel = this.m_world.CreateBody(bd);
            this.m_wheel.CreateFixture(fd);
        }
        {
            let jd:Box2D.Dynamics.Joints.b2RevoluteJointDef = new Box2D.Dynamics.Joints.b2RevoluteJointDef();
            let po:Box2D.Common.Math.b2Vec2 = pivot.Copy();
            po.Add(this.m_offset);
            jd.Initialize(this.m_wheel, this.m_chassis,po);
            jd.collideConnected = false;
            jd.motorSpeed = this.m_motorSpeed;
            jd.maxMotorTorque = 400;
            jd.enableMotor = this.m_motorOn;
            this.m_motorJoint = this.m_world.CreateJoint(jd) as Box2D.Dynamics.Joints.b2RevoluteJoint;
            
        }
        let wheelAnchor:Box2D.Common.Math.b2Vec2;
        wheelAnchor = new Box2D.Common.Math.b2Vec2(0, 24 / this.tScale);
        wheelAnchor.Add(pivot);

        this.CreateLeg(-1, wheelAnchor);
        this.CreateLeg(1, wheelAnchor);

        this.m_wheel.SetPositionAndAngle(this.m_wheel.GetPosition(), 120.0 * Math.PI / 180);
        this.CreateLeg(-1, wheelAnchor);
        this.CreateLeg(1, wheelAnchor);
        
        this.m_wheel.SetPositionAndAngle(this.m_wheel.GetPosition(), -120.0 * Math.PI / 180);
        this.CreateLeg(-1, wheelAnchor);
        this.CreateLeg(1, wheelAnchor);
    }

    private CreateLeg(s:number, wheelAnchor:Box2D.Common.Math.b2Vec2):void
    {
        let p1:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(162 * s / this.tScale, 183  / this.tScale);
        let p2:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(216 * s / this.tScale, 36  / this.tScale);
        let p3:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(129 * s / this.tScale, 57  / this.tScale);
        let p4:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(93 * s / this.tScale, -24  / this.tScale);
        let p5:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(180 * s / this.tScale, -45  / this.tScale);
        let p6:Box2D.Common.Math.b2Vec2 = new Box2D.Common.Math.b2Vec2(75 * s / this.tScale, -111  / this.tScale);

        let sd1:Box2D.Collision.Shapes.b2PolygonShape = new Box2D.Collision.Shapes.b2PolygonShape();
        let sd2:Box2D.Collision.Shapes.b2PolygonShape = new Box2D.Collision.Shapes.b2PolygonShape();
        let fd1:Box2D.Dynamics.b2FixtureDef = new Box2D.Dynamics.b2FixtureDef();
        let fd2:Box2D.Dynamics.b2FixtureDef = new Box2D.Dynamics.b2FixtureDef();

        fd1.shape = sd1;
        fd2.shape = sd2;
        fd1.filter.groupIndex = -1;
        fd2.filter.groupIndex = -1;
        fd1.density = 1;
        fd2.density = 1;

        if(s > 0)
        {
            sd1.SetAsArray([p3,p2,p1]);
            sd2.SetAsArray([
                Box2D.Common.Math.b2Math.SubtractVV(p6, p4),
                Box2D.Common.Math.b2Math.SubtractVV(p5, p4),
                new Box2D.Common.Math.b2Vec2()

            ])
        }
        else
        {
            sd1.SetAsArray([p2,p3,p1]);
            sd2.SetAsArray([
                Box2D.Common.Math.b2Math.SubtractVV(p5, p4),
                Box2D.Common.Math.b2Math.SubtractVV(p6, p4),
                new Box2D.Common.Math.b2Vec2()

            ])
        }

        var bd1:Box2D.Dynamics.b2BodyDef = new Box2D.Dynamics.b2BodyDef();
        bd1.type = Box2D.Dynamics.b2Body.b2_dynamicBody;
        var bd2:Box2D.Dynamics.b2BodyDef = new Box2D.Dynamics.b2BodyDef();
        bd2.type = Box2D.Dynamics.b2Body.b2_dynamicBody;

        bd1.position.SetV(this.m_offset);
        bd2.position = Box2D.Common.Math.b2Math.AddVV(p4, this.m_offset);

        bd1.angularDamping = 10;
        bd2.angularDamping = 10;

        let body1:Box2D.Dynamics.b2Body = this.m_world.CreateBody(bd1);
        let body2:Box2D.Dynamics.b2Body = this.m_world.CreateBody(bd2);

        body1.CreateFixture(fd1);
        body2.CreateFixture(fd2);

        var djd:Box2D.Dynamics.Joints.b2DistanceJointDef = new Box2D.Dynamics.Joints.b2DistanceJointDef();

        djd.dampingRatio = 0.5;
        djd.frequencyHz = 10;

        djd.Initialize(body1, body2, Box2D.Common.Math.b2Math.AddVV(p2, this.m_offset),Box2D.Common.Math.b2Math.AddVV(p5, this.m_offset));
        this.m_world.CreateJoint(djd);
        djd.Initialize(body1, body2, Box2D.Common.Math.b2Math.AddVV(p3, this.m_offset),Box2D.Common.Math.b2Math.AddVV(p4, this.m_offset));
        this.m_world.CreateJoint(djd);
        djd.Initialize(body1, this.m_wheel, Box2D.Common.Math.b2Math.AddVV(p3, this.m_offset),Box2D.Common.Math.b2Math.AddVV(wheelAnchor, this.m_offset));
        this.m_world.CreateJoint(djd);
        djd.Initialize(body2, this.m_wheel, Box2D.Common.Math.b2Math.AddVV(p6, this.m_offset),Box2D.Common.Math.b2Math.AddVV(wheelAnchor, this.m_offset));
        this.m_world.CreateJoint(djd);

        let rjd:Box2D.Dynamics.Joints.b2RevoluteJointDef = new Box2D.Dynamics.Joints.b2RevoluteJointDef();

        rjd.Initialize(body2, this.m_chassis, Box2D.Common.Math.b2Math.AddVV(p4, this.m_offset));
        this.m_world.CreateJoint(rjd);
    }

    public update():void
    {
        if(Input.isKeyPress(65))//a
        {
            this.m_chassis.SetAwake(true);
            this.m_motorJoint.SetMotorSpeed( -this.m_motorSpeed);

        }
        if(Input.isKeyPress(83))//s
        {
            this.m_chassis.SetAwake(true);
            this.m_motorJoint.SetMotorSpeed(0);
            
        }
        if(Input.isKeyPress(68))//d
        {
            this.m_chassis.SetAwake(true);
            this.m_motorJoint.SetMotorSpeed( this.m_motorSpeed);
            
        }
        if(Input.isKeyPress(77)) //m
        {
            this.m_chassis.SetAwake(true);
            this.m_motorJoint.EnableMotor(!this.m_motorJoint.IsMotorEnabled());
            
        }
        super.update();
    }
}